{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Generation of Motion Primitives\n",
    "\n",
    "This piece of code demonstrates how the motion primitves used in the search problem are generated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# add directories\n",
    "import sys\n",
    "sys.path.append(\"../../GSMP/motion_automata/\")\n",
    "sys.path.append(\"../../GSMP/motion_automata/vehicle_model/\")\n",
    "sys.path.append(\"../../GSMP/tools/\")\n",
    "\n",
    "import os\n",
    "import numpy as np\n",
    "import itertools\n",
    "from math import atan2, sin, cos\n",
    "import xml.etree.ElementTree as et\n",
    "\n",
    "# import vehicle model (https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/tree/master/Python)\n",
    "from vehicleDynamics_KS import vehicleDynamics_KS\n",
    "from parameters_vehicle1 import parameters_vehicle1\n",
    "from parameters_vehicle2 import parameters_vehicle2\n",
    "from parameters_vehicle3 import parameters_vehicle3\n",
    "\n",
    "# import solution checker\n",
    "from solution_checker import *\n",
    "\n",
    "from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper function to create a Trajectory object from a list of states.\n",
    "\n",
    "The elements of the states are: x, y, steering angle, velocity, orientation, time step"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_trajectory_from_states(list_states):\n",
    "    # list to hold states for final trajectory\n",
    "    list_states_new = list()\n",
    "    \n",
    "    # iterate through trajectory states\n",
    "    for state in list_states:\n",
    "        # feed in required slots\n",
    "        kwarg = {'position': np.array([state[0], state[1]]),\n",
    "                 'velocity': state[3],\n",
    "                 'steering_angle': state[2], \n",
    "                 'orientation': state[4],\n",
    "                 'time_step': state[5].astype(int)}\n",
    "\n",
    "        # append state\n",
    "        list_states_new.append(State(**kwarg))         \n",
    "        \n",
    "    # create new trajectory for evaluation\n",
    "    trajectory_new = Trajectory(initial_time_step=0, state_list=list_states_new)\n",
    "    \n",
    "    return trajectory_new"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper function to check the validity of a given trajectory.\n",
    "\n",
    "The validity is checked with the help of a solution checher under Kinematic Single-Track Model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def check_validity(trajectory_input, veh_type):\n",
    "    csw = CommonRoadSolutionWriter(output_dir=os.getcwd(),              \n",
    "                                   scenario_id=0,                 \n",
    "                                   step_size=0.1,                          \n",
    "                                   vehicle_type=veh_type,\n",
    "                                   vehicle_model=VehicleModel.KS,\n",
    "                                   cost_function=CostFunction.JB1)\n",
    "\n",
    "    # use solution writer to generate target xml file\n",
    "    csw.add_solution_trajectory(trajectory=trajectory_input, planning_problem_id=100)\n",
    "    xmlTree = csw.root_node\n",
    "\n",
    "    # generate states to be checked\n",
    "    [node] = xmlTree.findall('ksTrajectory')\n",
    "    veh_trajectory = KSTrajectory.from_xml(node)\n",
    "    veh_model = KinematicSingleTrackModel(veh_type_id, veh_trajectory, None)\n",
    "\n",
    "    # validate\n",
    "    result = TrajectoryValidator.is_trajectory_valid(veh_trajectory, veh_model, 0.1)\n",
    "    \n",
    "    return result"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Congiguration of parameters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# settings\n",
    "\n",
    "flag_print_info = False\n",
    "\n",
    "# vehicle parameter\n",
    "# 1: FORD_ESCORT 2: BMW_320i 3: VW_VANAGON\n",
    "veh_type_id = 3\n",
    "\n",
    "if veh_type_id == 1:\n",
    "    veh_type = VehicleType.FORD_ESCORT\n",
    "    veh_param = parameters_vehicle1()\n",
    "    veh_name = \"FORD_ESCORT\"\n",
    "elif veh_type_id == 2:\n",
    "    veh_type = VehicleType.BMW_320i\n",
    "    veh_param = parameters_vehicle2()\n",
    "    veh_name = \"BMW320i\"\n",
    "elif veh_type_id == 3:\n",
    "    veh_type = VehicleType.VW_VANAGON\n",
    "    veh_param = parameters_vehicle3()\n",
    "    veh_name = \"VW_VANAGON\"\n",
    "\n",
    "# total length of trajectory, in seconds\n",
    "T = 0.5\n",
    "# time step for states, in seconds\n",
    "# commonroad scenarios have dt of 0.1 seconds; for higher accuracy of forward simulation, dt here is set to 0.05\n",
    "# the simulated states will be down-sampled\n",
    "dt = 0.05\n",
    "# calculate time_stamps， *100 for 2 digits accuracy\n",
    "time_stamps = ((np.arange(0, T, dt) + dt) * 100).astype(int)\n",
    "\n",
    "# sampling range\n",
    "# in m/s\n",
    "min_range_sample_velocity = 0.0\n",
    "max_range_sample_velocity = 20.0\n",
    "num_sample_velocity = 20 + 1 # step v is then 1.0 m/s\n",
    "\n",
    "# steer to one side only, we can mirror the primitives afterwards\n",
    "# in rad\n",
    "min_range_sample_steering_angle = 0\n",
    "max_range_sample_steering_angle = veh_param.steering.max\n",
    "num_sample_steering_angle = 8 + 1 # step is roughly 10 degrees"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create list of possible samples for velocity and steering angle\n",
    "list_samples_v = np.linspace(min_range_sample_velocity, \n",
    "                             max_range_sample_velocity, \n",
    "                             num_sample_velocity)\n",
    "\n",
    "list_samples_steering_angle = np.linspace(min_range_sample_steering_angle, \n",
    "                                         max_range_sample_steering_angle,\n",
    "                                         num_sample_steering_angle)\n",
    "\n",
    "# for some statistics\n",
    "num_possible_states = num_sample_velocity * num_sample_steering_angle\n",
    "num_possible_start_end_combinations = num_possible_states ** 2\n",
    "count_processed = 0\n",
    "count_validated =0\n",
    "count_accepted = 0\n",
    "\n",
    "# for saving the results\n",
    "list_traj_accepted = []\n",
    "list_traj_failed = []\n",
    "\n",
    "print(\"Total possible combination of states: \", num_possible_start_end_combinations)\n",
    "\n",
    "# v = velocity, d = delta = steering_angle\n",
    "# iterate through possible instance of the cartesian product of list_samples_v and list_samples_steering_angle\n",
    "# create all possible combinations of (v_start, d_start) and (v_end, d_end)\n",
    "\n",
    "for (v_start, d_start) in itertools.product(list_samples_v, list_samples_steering_angle):\n",
    "    for (v_end, d_end) in itertools.product(list_samples_v, list_samples_steering_angle):\n",
    "        \n",
    "        count_processed += 1\n",
    "\n",
    "        # Print progress\n",
    "        if count_processed % 2000 == 0 and count_processed:\n",
    "            print(\"Progress: {} primitives checked.\".format(count_processed))\n",
    "            \n",
    "        # compute required inputs\n",
    "        a_input = (v_end - v_start) / T\n",
    "        steering_rate_input = (d_end - d_start) / T\n",
    "        \n",
    "        # check if the constraints are respected\n",
    "        if (a_input > veh_param.longitudinal.a_max) or (a_input < -veh_param.longitudinal.a_max) or \\\n",
    "           (steering_rate_input > veh_param.steering.v_max) or (steering_rate_input < -veh_param.steering.v_max):\n",
    "            continue\n",
    "        \n",
    "        if flag_print_info:\n",
    "            print(\"{:^8}, {:^8}, {:^8}, {:^8}, {:^8}\".format(\"x\", \"y\",\"steer\", \"v\", \"theta\"))\n",
    "            print(\"{:=<46}\".format(\"\"))\n",
    "\n",
    "        # list to store the states\n",
    "        list_states = []\n",
    "        \n",
    "        # trajectory always starts at position (0, 0) m with orientation of 0 rad\n",
    "        x_input = np.array([0.0, 0.0, d_start, v_start, 0.0])\n",
    "        u_input = np.array([steering_rate_input, a_input])\n",
    "\n",
    "        # time stamp = 0\n",
    "        list_states.append(np.append(x_input, 0))\n",
    "        \n",
    "        flag_friction_constraint_satisfied = True\n",
    "        \n",
    "        # forward simulation of states\n",
    "        # ref: https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf, page 4\n",
    "        for time_stamp in time_stamps:\n",
    "            # simulate state transition\n",
    "            x_dot = np.array(vehicleDynamics_KS(x_input, u_input, veh_param))\n",
    "            \n",
    "            # check friction circle constraint\n",
    "            if (a_input ** 2 + (x_input[3] * x_dot[4]) ** 2) ** 0.5 > veh_param.longitudinal.a_max:\n",
    "                flag_friction_constraint_satisfied = False\n",
    "                break\n",
    "            \n",
    "            # generate new state\n",
    "            x_output = x_input + x_dot * dt\n",
    "            \n",
    "            # subsample the states with step size of 0.1 seconds\n",
    "            if time_stamp % 10 == 0:\n",
    "                # add state to list\n",
    "                list_states.append(np.append(x_output, time_stamp / 10)) \n",
    "            \n",
    "            if flag_print_info:\n",
    "                print(\"{:^8.3f}, {:^8.3f}, {:^8.3f}, {:^8.3f}, {:^8.3f}\".format(\n",
    "                    x_output[0], x_output[1], x_output[2], x_output[3], x_output[4]))\n",
    "             \n",
    "            # prepare for next iteration\n",
    "            x_input = x_output\n",
    "\n",
    "        # skip this trajectory if the friction constraint is not satisfied\n",
    "        if not flag_friction_constraint_satisfied:\n",
    "            continue    \n",
    "        \n",
    "        # create trajectory from the list of states\n",
    "        trajectory_new = create_trajectory_from_states(list_states)\n",
    "             \n",
    "        result = check_validity(trajectory_new, veh_type)\n",
    "        count_validated += 1\n",
    "        \n",
    "        if result: \n",
    "            count_accepted += 1\n",
    "            list_traj_accepted.append(trajectory_new)\n",
    "\n",
    "print(\"============================================\")\n",
    "\n",
    "if count_validated != 0:\n",
    "    percentage_accept = round(count_accepted / count_validated, 2) * 100\n",
    "else:\n",
    "    percentage_accept = 0\n",
    "    \n",
    "print(\"Validated: {}, Accepted: {}, Rate: {}%\".format(count_validated, count_accepted, percentage_accept))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Plot generate motion primitives."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "for traj in list_traj_accepted:\n",
    "\n",
    "    list_x = [state.position[0] for state in traj.state_list]\n",
    "    list_y = [state.position[1] for state in traj.state_list]\n",
    "            \n",
    "    plt.plot(list_x, list_y)\n",
    "    \n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create mirrored primitives"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# make mirrored ones\n",
    "import copy\n",
    "\n",
    "# make sure to make a deep copy\n",
    "list_traj_accepted_mirrored = copy.deepcopy(list_traj_accepted)\n",
    "\n",
    "count_acc = 0\n",
    "for traj in list_traj_accepted:\n",
    "    list_states_mirrored = []\n",
    "    \n",
    "    for state in traj.state_list:\n",
    "        # add mirrored state into list\n",
    "        list_states_mirrored.append([state.position[0],\n",
    "                                    -state.position[1],\n",
    "                                    -state.steering_angle,\n",
    "                                     state.velocity,\n",
    "                                    -state.orientation,\n",
    "                                     state.time_step])\n",
    "    \n",
    "    trajectory_new = create_trajectory_from_states(list_states_mirrored)\n",
    "    \n",
    "    # double check the validity before adding to the list\n",
    "    if check_validity(trajectory_new, veh_type):\n",
    "        list_traj_accepted_mirrored.append(trajectory_new)\n",
    "        count_acc += 1\n",
    "\n",
    "print(\"Total number of primitives (mirrored included): \", len(list_traj_accepted_mirrored))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### check average number of successors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "list_count_seccesors = []\n",
    "for prim_main in list_traj_accepted_mirrored:\n",
    "    count_successors = 0\n",
    "    \n",
    "    for prim_2bc in list_traj_accepted_mirrored:\n",
    "        \n",
    "        state_final_prim_main = prim_main.state_list[-1]\n",
    "        state_initial_prim_2bc = prim_2bc.state_list[0]\n",
    "        \n",
    "        if abs(state_final_prim_main.velocity - state_initial_prim_2bc.velocity) < 0.02 and \\\n",
    "           abs(state_final_prim_main.steering_angle - state_initial_prim_2bc.steering_angle) < 0.02:\n",
    "            count_successors += 1\n",
    "    \n",
    "    list_count_seccesors.append(count_successors)\n",
    "\n",
    "print(\"average number of successors: \", np.mean(list_count_seccesors))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Plot final primitives"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "fig = plt.figure()\n",
    "\n",
    "for i in range(len(list_traj_accepted_mirrored)):\n",
    "    traj = list_traj_accepted_mirrored[i]\n",
    "\n",
    "    list_x = [state.position[0] for state in traj.state_list]\n",
    "    list_y = [state.position[1] for state in traj.state_list]\n",
    "    \n",
    "    # length constraint\n",
    "    x_start, y_start = list_x[0], list_y[0]\n",
    "    x_end, y_end = list_x[-1], list_y[-1]\n",
    "\n",
    "    # only plot trajectories that are not longer than the threshold\n",
    "    if np.linalg.norm([x_end - x_start, y_end - y_start]) > 20:\n",
    "        continue \n",
    "    \n",
    "    plt.plot(list_x, list_y)\n",
    "    \n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Generate sample trajectories and check if they pass the check"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "list_traj_accepted_mirrored_backup = copy.deepcopy(list_traj_accepted_mirrored)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# generate sample path and check if they pass the solution checker\n",
    "import random\n",
    "import copy\n",
    "random.seed()\n",
    "\n",
    "# number of primitives to be connected\n",
    "num_seg_path = 10\n",
    "num_simulation = 200\n",
    "for count_run in range(num_simulation):\n",
    "    print(count_run)\n",
    "    count_seg_path = 0\n",
    "\n",
    "    num_prim = len(list_traj_accepted_mirrored)\n",
    "    # get a random start primitive id\n",
    "    idx_prim = random.randrange(num_prim)\n",
    "    list_trajectories = []\n",
    "\n",
    "    while count_seg_path < num_seg_path:\n",
    "        # retrieve primitive\n",
    "        prim_main = copy.deepcopy(list_traj_accepted_mirrored[idx_prim])\n",
    "        list_trajectories.append(prim_main)\n",
    "        list_successors_prim_main = []\n",
    "        count_seg_path += 1\n",
    "\n",
    "        # obtain its successors\n",
    "        for j in range(num_prim):\n",
    "            prim_2bc = list_traj_accepted_mirrored[j]\n",
    "\n",
    "            state_final_prim_main = prim_main.state_list[-1]\n",
    "            state_initial_prim_2bc = prim_2bc.state_list[0]\n",
    "\n",
    "            if abs(state_final_prim_main.velocity - state_initial_prim_2bc.velocity) < 0.02 and \\\n",
    "               abs(state_final_prim_main.steering_angle - state_initial_prim_2bc.steering_angle) < 0.02:\n",
    "\n",
    "                list_successors_prim_main.append(j)\n",
    "\n",
    "        # start over if a primitive does not have a valid successor\n",
    "        num_successors_prim_main = len(list_successors_prim_main)\n",
    "        if num_successors_prim_main == 0:\n",
    "            count_seg_path = 0\n",
    "            idx_prim = random.randrange(num_prim)\n",
    "            list_trajectories = []\n",
    "        else:\n",
    "            # else add a random succesor into list of prims\n",
    "            idx_successor = random.randrange(num_successors_prim_main)\n",
    "            idx_prim = list_successors_prim_main[idx_successor]\n",
    "\n",
    "#     fig = plt.figure()\n",
    "\n",
    "    # plot first prim\n",
    "    list_states_final = copy.deepcopy(list_trajectories[0].state_list)\n",
    "    list_x = [state.position[0] for state in list_states_final]\n",
    "    list_y = [state.position[1] for state in list_states_final]\n",
    "    plt.scatter(list_x, list_y)\n",
    "\n",
    "    list_trajectories_backup = copy.deepcopy(list_trajectories)\n",
    "\n",
    "    # plot remaining prims\n",
    "    for i in range(1, len(list_trajectories)):\n",
    "\n",
    "        traj_pre = list_trajectories[i - 1]\n",
    "        traj_cur = list_trajectories[i]   \n",
    "\n",
    "        # retrieve states\n",
    "        state_final_traj_pre   = traj_pre.state_list[-1]\n",
    "        state_initial_traj_cur = traj_cur.state_list[0]\n",
    "\n",
    "        while not(state_initial_traj_cur.orientation < 0.001):\n",
    "            print(\"error in orientation\", state_initial_traj_cur.orientation, i)\n",
    "            traj_cur.translate_rotate(np.zeros(2), -state_initial_traj_cur.orientation)\n",
    "            state_initial_traj_cur = traj_cur.state_list[0]\n",
    "            print(state_initial_traj_cur.orientation)\n",
    "\n",
    "        while not (state_initial_traj_cur.position[0] < 0.001 and state_initial_traj_cur.position[1] < 0.001):\n",
    "            print(\"error in position\", state_initial_traj_cur.position, i)\n",
    "            traj_cur.translate_rotate(-state_initial_traj_cur.position, 0)\n",
    "            state_initial_traj_cur = traj_cur.state_list[0]\n",
    "            print(state_initial_traj_cur.position)\n",
    "\n",
    "        # rotate + translate with regard to the last state of preivous trajectory\n",
    "        traj_cur.translate_rotate(np.zeros(2), state_final_traj_pre.orientation)\n",
    "        traj_cur.translate_rotate(state_final_traj_pre.position, 0)\n",
    "\n",
    "        # retrieve new states\n",
    "        state_final_traj_pre   = traj_pre.state_list[-1]\n",
    "        state_initial_traj_cur = traj_cur.state_list[0]\n",
    "\n",
    "        list_x = [state.position[0] for state in traj_cur.state_list]\n",
    "        list_y = [state.position[1] for state in traj_cur.state_list]\n",
    "\n",
    "        plt.scatter(list_x, list_y)\n",
    "\n",
    "        # discard the first state of second primitive onward\n",
    "        traj_cur.state_list.pop(0)\n",
    "        list_states_final += traj_cur.state_list\n",
    "\n",
    "\n",
    "    list_x = [state.position[0] for state in list_states_final]\n",
    "    list_y = [state.position[1] for state in list_states_final]\n",
    "\n",
    "    plt.xlim([min(list_x) - 2, max(list_x) + 2])\n",
    "    plt.ylim([min(list_y) - 2, max(list_y) + 2])\n",
    "    plt.axis('equal')\n",
    "#     plt.show()\n",
    "\n",
    "     # save as cr node to be validated via the solution checker\n",
    "\n",
    "    list_states_for_traj = []\n",
    "    count = 0\n",
    "    for state in list_states_final:\n",
    "        list_states_for_traj.append([state.position[0],\n",
    "                                   state.position[1],\n",
    "                                   state.steering_angle,\n",
    "                                   state.velocity,\n",
    "                                   state.orientation,\n",
    "                                   np.int64(count)])\n",
    "        count += 1\n",
    "\n",
    "    trajectory_new = create_trajectory_from_states(list_states_for_traj)\n",
    "    result = check_validity(trajectory_new, veh_type)\n",
    "#     print(\"Validation result of sample path: \", result)\n",
    "    if not result:\n",
    "        print(\"This is not good :(\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### save the result as xml file"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create Trajectories tag\n",
    "node_trajectories = et.Element('Trajectories')\n",
    "\n",
    "for trajectory in list_traj_accepted_mirrored:\n",
    "# trajectory = list_traj_accepted_mirrored[137]\n",
    "\n",
    "    # create a tag for individual trajectory\n",
    "    node_trajectory = et.SubElement(node_trajectories, 'Trajectory')\n",
    "\n",
    "    # add time duration tag\n",
    "    node_duration = et.SubElement(node_trajectory, 'Duration')\n",
    "    node_duration.set('unit','deci-second')\n",
    "    node_duration.text = \"10\"\n",
    "\n",
    "    list_states = trajectory.state_list\n",
    "\n",
    "    # add start state\n",
    "    node_start = et.SubElement(node_trajectory, 'Start')\n",
    "    node_x = et.SubElement(node_start, 'x')\n",
    "    node_y = et.SubElement(node_start, 'y')\n",
    "    node_sa = et.SubElement(node_start, 'steering_angle')\n",
    "    node_v = et.SubElement(node_start, 'velocity')\n",
    "    node_o = et.SubElement(node_start, 'orientation')\n",
    "    node_t = et.SubElement(node_start, 'time_step')\n",
    "\n",
    "    state_start = list_states[0]\n",
    "\n",
    "    node_x.text = str(state_start.position[0])\n",
    "    node_y.text = str(state_start.position[1])\n",
    "    node_sa.text = str(state_start.steering_angle)\n",
    "    node_v.text = str(state_start.velocity)\n",
    "    node_o.text = str(state_start.orientation)\n",
    "    node_t.text = str(state_start.time_step)\n",
    "\n",
    "    # add final state\n",
    "    node_final = et.SubElement(node_trajectory, 'Final')\n",
    "    node_x = et.SubElement(node_final, 'x')\n",
    "    node_y = et.SubElement(node_final, 'y')\n",
    "    node_sa = et.SubElement(node_final, 'steering_angle')\n",
    "    node_v = et.SubElement(node_final, 'velocity')\n",
    "    node_o = et.SubElement(node_final, 'orientation')\n",
    "    node_t = et.SubElement(node_final, 'time_step')\n",
    "\n",
    "    state_final = list_states[-1]\n",
    "\n",
    "    node_x.text = str(state_final.position[0])\n",
    "    node_y.text = str(state_final.position[1])\n",
    "    node_sa.text = str(state_final.steering_angle)\n",
    "    node_v.text = str(state_final.velocity)\n",
    "    node_o.text = str(state_final.orientation)\n",
    "    node_t.text = str(state_final.time_step)\n",
    "\n",
    "    # add states in between\n",
    "    list_states_in_between = list_states[1:-1]\n",
    "\n",
    "    node_path = et.SubElement(node_trajectory, 'Path')\n",
    "\n",
    "    for state in list_states_in_between:\n",
    "        node_state = et.SubElement(node_path, 'State')\n",
    "\n",
    "        node_x = et.SubElement(node_state, 'x')\n",
    "        node_y = et.SubElement(node_state, 'y')\n",
    "        node_sa = et.SubElement(node_state, 'steering_angle')\n",
    "        node_v = et.SubElement(node_state, 'velocity')\n",
    "        node_o = et.SubElement(node_state, 'orientation')\n",
    "        node_t = et.SubElement(node_state, 'time_step')\n",
    "\n",
    "        node_x.text = str(state.position[0])\n",
    "        node_y.text = str(state.position[1])\n",
    "        node_sa.text = str(state.steering_angle)\n",
    "        node_v.text = str(state.velocity)\n",
    "        node_o.text = str(state.orientation)\n",
    "        node_t.text = str(state.time_step)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from xml.dom import minidom\n",
    "prefix = \"../../GSMP/motion_automata/motion_primitives/\"\n",
    "vtep = round((max_range_sample_velocity - min_range_sample_velocity) / (num_sample_velocity - 1), 2)\n",
    "sastep = round(max_range_sample_steering_angle * 2 / (num_sample_steering_angle - 1), 2)\n",
    "file_name = \"V_{}_{}_Vstep_{}_SA_{}_{}_SAstep_{}_T_{}_Model_{}.xml\".format(min_range_sample_velocity, max_range_sample_velocity,\n",
    "                                                                           vtep,\n",
    "                                                                           -max_range_sample_steering_angle, max_range_sample_steering_angle,\n",
    "                                                                           sastep,\n",
    "                                                                           round(T, 1), \n",
    "                                                                           veh_name)\n",
    "\n",
    "xml_prettified = minidom.parseString(et.tostring(node_trajectories)).toprettyxml(indent=\"   \")\n",
    "with open(prefix + file_name, \"w\") as f:\n",
    "    f.write(xml_prettified)\n",
    "    print(\"file saved: {}\".format(file_name))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python (cr37)",
   "language": "python",
   "name": "cr37"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
